

import rospy
import threading
from geometry_msgs.msg import Twist

Omni = 0 #全向移动模式

import time

class sp_ctl:
    def __init__(self) -> None:
    
        rospy.init_node('turtlebot_teleop') #创建ROS节点
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5) #创建速度话题发布者，'~cmd_vel'='节点名/cmd_vel'
        self.spd=[0,0,0]
        self.lock     =threading. Lock()
        thread1 = threading.Thread(target=self.thread_com_receive)
        thread1.start()
    def thread_com_receive(self):
        cnt=0
        while 1:
            time.sleep(0.03)
            cnt+=1
            with self.lock:
                spd=self.spd
            self.ctl(*spd)
            # print(cnt,spd[0],spd[1],spd[2])
    def ctl(self,x,y,z):
        twist = Twist()
        twist.linear.x  = x; twist.linear.y = y;  twist.linear.z = 0
        twist.angular.x = 0;twist.angular.y = 0; twist.angular.z = z
        # print(twist)
        self.pub.publish(twist) #ROS发布速度话题
    def set(self,x,y,z):
        with self.lock:
            self.spd=[x,y,z]
    def reset(self):
        twist = Twist()
        twist.linear.x = 0;  twist.linear.y = 0;  twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        self.pub.publish(twist)


